﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Woodstock.HAL.Phidgets;
using Phidgets;
using System.Threading;
using Woodstock.Common;
using System.Collections.ObjectModel;
using Telerik.Windows.Controls.Charting;
using System.Windows.Threading;
using System.Windows.Media.Media3D;

namespace Woodstock.Quad.TestApp
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {

        public MainWindow()
        {
            InitializeComponent();
        }

        QuadFlightControl quad;





        ObservableCollection<SpatialState> SpatialStateHistory;

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            SpatialStateHistory = new ObservableCollection<SpatialState>();

            PhidgetSpatial spatialProvider = new PhidgetSpatial();
            spatialProvider.SpatialStateChanged += new EventHandler<Common.SpatialStateEventArgs>(spatialProvider_SpatialStateChanged);

            quad = new QuadFlightControl(spatialProvider, 
                new PhidgetServoMotor(0, 2000, 1000),
                new PhidgetServoMotor(1, 2000, 1000),
                new PhidgetServoMotor(2, 2000, 1000),
                new PhidgetServoMotor(3, 2000, 1000));


            Binding globalSpeedBinding = new Binding();
            globalSpeedBinding.Source = quad;
            globalSpeedBinding.Path = new PropertyPath("GlobalMotorSpeed");
            globalSpeedBinding.Mode = BindingMode.TwoWay;
            textBoxGlobalSpeed.SetBinding(TextBox.TextProperty, globalSpeedBinding);

            Binding motor1SpeedBinding = new Binding();
            motor1SpeedBinding.Source = quad.Motor1;
            motor1SpeedBinding.Path = new PropertyPath("Position");
            motor1SpeedBinding.Converter = new DoubleToStringConverter();
            motor1SpeedBinding.Mode = BindingMode.TwoWay;
            textBoxSpeedMotor1.SetBinding(TextBox.TextProperty, motor1SpeedBinding);

            Binding motor2SpeedBinding = new Binding();
            motor2SpeedBinding.Source = quad.Motor2;
            motor2SpeedBinding.Path = new PropertyPath("Position");
            motor2SpeedBinding.Converter = new DoubleToStringConverter();
            motor2SpeedBinding.Mode = BindingMode.TwoWay;
            textBoxSpeedMotor2.SetBinding(TextBox.TextProperty, motor2SpeedBinding);

            Binding motor3SpeedBinding = new Binding();
            motor3SpeedBinding.Source = quad.Motor3;
            motor3SpeedBinding.Path = new PropertyPath("Position");
            motor3SpeedBinding.Converter = new DoubleToStringConverter();
            motor3SpeedBinding.Mode = BindingMode.TwoWay;
            textBoxSpeedMotor3.SetBinding(TextBox.TextProperty, motor3SpeedBinding);

            Binding motor4SpeedBinding = new Binding();
            motor4SpeedBinding.Source = quad.Motor4;
            motor4SpeedBinding.Path = new PropertyPath("Position");
            motor4SpeedBinding.Converter = new DoubleToStringConverter();
            motor4SpeedBinding.Mode = BindingMode.TwoWay;
            textBoxSpeedMotor4.SetBinding(TextBox.TextProperty, motor4SpeedBinding);

            textBlockMotorsAttached.Visibility = System.Windows.Visibility.Visible;

            //listViewStateHistory.ItemsSource = SpatialStateHistory;


            //Line acelerationLine = new Line();
            //acelerationLine.Stroke = new SolidColorBrush(Colors.Red);
            //gridAceleration.Children.Add(acelerationLine);


            //acelerationLine.X1 = acelerationLine.ActualWidth / 2;
            //acelerationLine.Y1 = acelerationLine.ActualHeight / 2;

            //Binding xBinding = new Binding();
            //xBinding.Source = quad.StateHistory;
            //xBinding.Path = new PropertyPath("XAcceleration");
            //xBinding.Mode = BindingMode.OneWay;

            //acelerationLine.SetBinding(Line.X2Property, xBinding);

            //acelerationLine.X2 = 20;
            //acelerationLine.Y2 = 20;



            
            SetUpRadChart();
            SetUpXAxis();
            
            SetUpRollYAxis();
            SetUpRollMapping();

            DispatcherTimer timer1 = new DispatcherTimer();
            timer1.Interval = TimeSpan.FromMilliseconds(500);
            timer1.Tick += new EventHandler(timer1_Tick);
            timer1.Start();

            //advServo.Attach += new global::Phidgets.Events.AttachEventHandler(advServo_Attach);
            //advServo.open(-1);

            radChartAngles.ItemsSource = SpatialStateHistory;


        }

        void timer1_Tick(object sender, EventArgs e)
        {
            //SetUpAxisXRange();

            lock (SpatialStateHistory)
            {
                //SetUpAxisXRange();

                //radChartAngles.ItemsSource = null;
                //radChartAngles.ItemsSource = SpatialStateHistory;
            }
        }

        #region Chart

        List<double[]> itemsSource;

        private void FillSampleChartData()
        {
            SeriesMapping sm1 = new SeriesMapping();
            BarSeriesDefinition d1 = new BarSeriesDefinition();
            sm1.SeriesDefinition = d1;
            sm1.LegendLabel = "Bar Series";
            sm1.CollectionIndex = 0;

            ItemMapping im1 = new ItemMapping();
            im1.DataPointMember = DataPointMember.YValue;
            sm1.ItemMappings.Add(im1);

            SeriesMapping sm2 = new SeriesMapping();
            SplineSeriesDefinition d2 = new SplineSeriesDefinition();
            sm2.SeriesDefinition = d2;
            sm2.LegendLabel = "Spline Series";
            sm2.CollectionIndex = 1;

            ItemMapping im2 = new ItemMapping();
            im2.DataPointMember = DataPointMember.YValue;
            sm2.ItemMappings.Add(im2);

            radChartAngles.SeriesMappings.Add(sm1);
            radChartAngles.SeriesMappings.Add(sm2);

            itemsSource = new List<double[]>();           

            this.radChartAngles.ItemsSource = itemsSource;
        }

        private void SetUpRadChart()
        {
            radChartAngles.DefaultView.ChartTitle.Content = "Server Load";
            radChartAngles.DefaultView.ChartArea.NoDataString = "Waiting for data...";
            radChartAngles.DefaultView.ChartArea.EnableAnimations = true;
        }

        private void SetUpXAxis()
        {
            /*radChartAngles.DefaultView.ChartArea.AxisX.DefaultLabelFormat = "#VAL{hh:mm:ss}";
            radChartAngles.DefaultView.ChartArea.AxisX.LabelRotationAngle = 90;
            //radChartAngles.DefaultView.ChartArea.AxisX.LabelStep = 2;
            radChartAngles.DefaultView.ChartArea.AxisX.Title = "Time";
            radChartAngles.DefaultView.ChartArea.AxisX.LayoutMode = AxisLayoutMode.Normal;
            radChartAngles.DefaultView.ChartArea.AxisX.AutoRange = true;*/
        }

        private void SetUpAxisXRange()
        {
            radChartAngles.DefaultView.ChartArea.AxisX.MinValue = SpatialStateHistory.Min(e => e.DateTime).ToOADate();
            radChartAngles.DefaultView.ChartArea.AxisX.MaxValue = SpatialStateHistory.Max(e => e.DateTime).ToOADate();
            radChartAngles.DefaultView.ChartArea.AxisX.Step = (radChartAngles.DefaultView.ChartArea.AxisX.MaxValue - radChartAngles.DefaultView.ChartArea.AxisX.MinValue) / 100;
        }

        private void SetUpRollYAxis()
        {
            radChartAngles.DefaultView.ChartArea.AxisY.AxisName = "Roll";
            radChartAngles.DefaultView.ChartArea.AxisY.Title = "Roll Angle";
            radChartAngles.DefaultView.ChartArea.AxisY.AutoRange = true;
            //rollAngleAxis.MinValue = 0;
            //rollAngleAxis.MaxValue = 360;
            //radChartAngles.DefaultView.ChartArea.AxisY.Step = 1;
            radChartAngles.DefaultView.ChartArea.AxisY.DefaultLabelFormat = "#VAL Degrees";         
        }

        private void SetUpRollMapping()
        {
            SeriesMapping rollDataMapping = new SeriesMapping();
            rollDataMapping.LegendLabel = "Roll";
            rollDataMapping.SeriesDefinition = new LineSeriesDefinition();
            (rollDataMapping.SeriesDefinition as LineSeriesDefinition).ShowPointMarks = false;
            (rollDataMapping.SeriesDefinition as LineSeriesDefinition).ShowItemLabels = false;
            rollDataMapping.SeriesDefinition.AxisName = "Roll";
            
            rollDataMapping.ItemMappings.Add(new ItemMapping("DateTime", DataPointMember.XValue));
            rollDataMapping.ItemMappings.Add(new ItemMapping("XAngle", DataPointMember.YValue));
            
            radChartAngles.SeriesMappings.Add(rollDataMapping);
        }

        #endregion

        private double DegreeToRadian(double angle)
        {
            return Math.PI * angle / 180.0;
        }

        public Quaternion QuartenionFromEulerAngles(double heading, double attitude, double bank) {
        // Assuming the angles are in radians.
            double c1 = Math.Cos(heading/2);
            double s1 = Math.Sin(heading/2);
            double c2 = Math.Cos(attitude/2);
            double s2 = Math.Sin(attitude/2);
            double c3 = Math.Cos(bank/2);
            double s3 = Math.Sin(bank/2);
            double c1c2 = c1*c2;
            double s1s2 = s1*s2;


            Quaternion result = new Quaternion();

            result.W =c1c2*c3 - s1s2*s3;
  	        result.X =c1c2*s3 + s1s2*c3;
            result.Y = s1 * c2 * c3 + c1 * s2 * s3;
            result.Z = c1 * s2 * c3 - s1 * c2 * s3;

            return result;
      }

        void spatialProvider_SpatialStateChanged(object sender, Common.SpatialStateEventArgs e)
        {
            /*
            lock (SpatialStateHistory)
            {
                //if (this.SpatialStateHistory.Count >= 100)
                 //   this.SpatialStateHistory.Dequeue();

                SpatialStateHistory.Add(e.State);
            }*/

            
            
            Dispatcher.Invoke(new Action(() => 
            {
                //PitchAngleRotation.Angle = e.State.XAngle * -1;
                //RollAngleRotation.Angle = e.State.YAngle;
                //YawAngleRotation.Angle = e.State.ZAngle * -1;

                Quaternion rotation = QuartenionFromEulerAngles(DegreeToRadian(e.State.ZAngle * -1), DegreeToRadian(e.State.XAngle * -1), DegreeToRadian(e.State.YAngle));




                Quaternion q1 = new Quaternion();
                
                q1.X = 0;
                q1.Y = 0;
                q1.Z = -1 * Math.Sin(DegreeToRadian(e.State.XAngle) / 2);
                q1.W = Math.Cos(DegreeToRadian(e.State.XAngle) / 2);


                Quaternion q2 = new Quaternion();

                q2.X = 1 * Math.Sin(DegreeToRadian(e.State.YAngle) / 2);
                q2.Y = 0;
                q2.Z = 0;
                q2.W = Math.Cos(DegreeToRadian(e.State.YAngle) / 2);

                Quaternion q3 = new Quaternion();

                q3.X = 0;
                q3.Y = -1 * Math.Sin(DegreeToRadian(e.State.ZAngle) / 2);
                q3.Z = 0;
                q3.W = Math.Cos(DegreeToRadian(e.State.ZAngle) / 2);

                //quartenion3d.Quaternion = Quaternion.Multiply(q3, Quaternion.Multiply(q1, q2));
                quartenion3d.Quaternion = rotation;

                //myAngleRotation2.Angle = e.State.Pitch;


                /*gridHorizon.Children.Clear();


                System.Windows.Shapes.Line line = new System.Windows.Shapes.Line();
                line.Width = 1;
                line.trans*/
                




                //SpatialStateHistory.Add(e.State);

                textBoxGyroX.Text = e.State.XAngle.ToString("00.00");
                textBoxGyroY.Text = e.State.YAngle.ToString("00.00");
                textBoxGyroZ.Text = e.State.ZAngle.ToString("00.00");

                //textBoxAcelX.Text = e.State.XAcceleration.ToString("00.000");
                //textBoxAcelY.Text = e.State.YAcceleration.ToString("00.000");
                //textBoxAcelZ.Text = e.State.ZAcceleration.ToString("00.000");
            }
            ));
            
        }




        private void Window_KeyDown(object sender, KeyEventArgs e)
        {
            switch (e.Key)
            {
                case Key.Add:
                    quad.GlobalMotorSpeed += 10;
                    break;

                case Key.Subtract:
                    quad.GlobalMotorSpeed -= 10;
                    break;

                default:
                    break;
            }
        }

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            quad.On = false;

            Thread.Sleep(1000);
        }

        private void buttonTurnOn_Click(object sender, RoutedEventArgs e)
        {
            quad.On = true;
        }
    }
}
